#include "motorsTask.h"

#include "FreeRTOS.h"
#include "task.h"

#include <math.h>
#include "nxbot.h"

static void motorsTask(void *pvParameters);
//int sum(int *buffer);

void executeMotorsTask(globals_t *pvParameter)
{
	xTaskCreate(motorsTask,(const signed portCHAR * const) "motorTask",motorsTask_stackSize,(void *)pvParameter,tskIDLE_PRIORITY+2,( xTaskHandle * ) NULL);
}


//Tarea para solo actualizar las vels en la esctructura global
static void motorsTask(void *pvParameters)
{	
	//convertion of parameters 
	globals_t *globals;
	globals=(globals_t *)pvParameters;

	unsigned int freq;
	portTickType actualTick;

	//odometers direction
	unsigned char leftMotorDir = 0, rightMotorDir = 0;

	
	#if USE_ODOMETRY==1
	//variables used for odometry
	long change_left;
  long change_right;
  float transchange;
  float rotchange;
  float dx,dy;
  #endif
  
	
	for(;;)
	{		

		//aquire the frequency for the task		
		
		freq=globals->times.intervalMotors;
		
		//First update position
		motors_getActualPos(&(globals->motors));
		
		//Update velocities
		motors_getActualVels(&(globals->motors.velocities), MOTOR_INTERVAL_HZ);
		
		//Odometry calculation
		#if USE_ODOMETRY==1
		//calculate variations of position of wheels
		change_left = globals->encoders.posLeft - globals->encoders.lastPosLeft;
  	change_right = globals->encoders.posRight - globals->encoders.lastPosRight;
  
		//correction of encoders overflow
		if(change_left>60000)
			change_left = (globals->encoders.posLeft-65535)-globals->encoders.lastPosLeft;
	 	else if(change_left<-60000)
	 		change_left = (globals->encoders.posLeft)+(65535-globals->encoders.lastPosLeft);
	 	
	 	if(change_right>60000)
			change_right = (globals->encoders.posRight-65535)-globals->encoders.lastPosRight;
	 	else if(change_right<-60000)
	 		change_right = (globals->encoders.posRight)+(65535-globals->encoders.lastPosRight);
 	
	 	//update last positions
		globals->encoders.lastPosLeft = globals->encoders.posLeft;
		globals->encoders.lastPosRight = globals->encoders.posRight;

		transchange = (change_left + change_right) * NXBOT_ENCODER_RES / 2;
		rotchange = (change_left - change_right) * NXBOT_ENCODER_RES / NXBOT_WHEEL_DISTANCE;

		dx = transchange*cos(-globals->grobotPose.yaw - rotchange/2);
		dy = transchange*sin(-globals->grobotPose.yaw - rotchange/2);
		
		globals->robotPose.x += dx;
		globals->robotPose.y += dy;
		globals->robotPose.yaw += rotchange;

		while (globals->robotPose.yaw > 2*M_PI) 
			globals->robotPose.yaw -= 2*M_PI;
		while (globals->robotPose.yaw < 0) 
			globals->robotPose.yaw += 2*M_PI;

		globals->robotPose.orientation = globals->robotPose.yaw;
		
		if(globals->robotPose.orientation > M_PI)
  		globals->robotPose.orientation -=2*M_PI;

  	globals->robotPose.orientation *= -1;
		#endif
		
		//define direction of odometers, in order to increment or decrement them
		if(globals->motors.velocities.velLeftRef>0)
			leftMotorDir = ENC_FORWARD;
		else if(globals->motors.velocities.velLeftRef<0)
			leftMotorDir = ENC_REVERSE;
			
		if(globals->motors.velocities.velRightRef>0)
			rightMotorDir = ENC_FORWARD;
		else if (globals->motors.velocities.velRightRef<0)
			rightMotorDir = ENC_REVERSE;
		
		//Set direction in odometers
		if(globals->motors.velocities.velLeftRef || globals->motors.velocities.velRightRef)  
			motors_setDirection(leftMotorDir, rightMotorDir);
		
		//then set velocity in motors
		motors_setVel(globals->motors.velocities.velLeftRef,MOTOR_LEFT);
		motors_setVel(globals->motors.velocities.velRightRef,MOTOR_RIGHT);
		
		//time delay according to the encodersInterval value
		actualTick = xTaskGetTickCount();
		vTaskDelayUntil( &actualTick,(portTickType)freq);		
	}
}

/*
int sum(int *buffer)
{
	int result;
	for(char i=0; i<8; i++)
		result += buffer[i];
	
	return result;
		
}
*/
